#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rospy
import moveit_commander
import sys
from tf.transformations import quaternion_matrix

# 声明逆运动学函数
def compute_ik(group, pose):
    position = pose[:3]
    orientation = pose[3:]

    # 构建末端执行器的变换矩阵
    transformation_matrix = quaternion_matrix(orientation)
    transformation_matrix[:3, 3] = position

    pose_goal = group.get_current_pose().pose
    pose_goal.position.x = position[0]
    pose_goal.position.y = position[1]
    pose_goal.position.z = position[2]
    pose_goal.orientation.x = orientation[0]
    pose_goal.orientation.y = orientation[1]
    pose_goal.orientation.z = orientation[2]
    pose_goal.orientation.w = orientation[3]

    group.set_pose_target(pose_goal)
    success, plan, _, _ = group.plan()

    if success and plan.joint_trajectory.points:
        joint_values = plan.joint_trajectory.points[-1].positions
        return joint_values
    else:
        rospy.loginfo("Did not find IK solution")
        return []

def main():
    rospy.init_node("kdl_kinematics_example", anonymous=True)
    moveit_commander.roscpp_initialize(sys.argv)
    
    robot = moveit_commander.RobotCommander()
    group = moveit_commander.MoveGroupCommander("manipulator")

    # 起始点
    input_pose_start = [0.00627459278920944, 0.011697048892667358, 0.1426654620685569, 
                        0.9063306950280158, -0.42256913167865406, -1.1063811351183932e-05, 8.942841225197472e-06]
    # 目标点
    input_pose_end = [0.006294315226467101, 0.28918579303355113, 0.2502609827289403, 
                      0.9999999984636202, 4.917028402675271e-05, 3.6721275695023544e-06, -2.532900424562415e-05]

    joint_values_start = compute_ik(group, input_pose_start)
    joint_values_end = compute_ik(group, input_pose_end)

    for i, joint_value in enumerate(joint_values_start):
        rospy.loginfo(f"Joint_start {i}: {joint_value}")

    for i, joint_value in enumerate(joint_values_end):
        rospy.loginfo(f"Joint_end {i}: {joint_value}")

    moveit_commander.roscpp_shutdown()

if __name__ == "__main__":
    main()
